function r = boatmen_model(pIn,Cd_app,Cd_body)
% ODE model of boatmen hydrodynamics.
% This is a modified version of Blake, 1985
% pIn - Inout parameters required to run a simulation
% r   - structure of simulaton results
% sim_mode - ('optimize','default')


%% Input parameter values

% Body length (m) 
L = pIn.body_len;

% Length of the paddle (m)
pLen = pIn.app_len;

% Number of strokes
%num_strokes = pIn.num_strokes;

% Parameter defaults
if nargin < 3  
    % Drag coefficient of the body
    Cd_body = 1.07;
    
    if nargin < 2
        % Drag coefficient of the appendage
        Cd_app = 1.1;
         
    end
end

% Body mass (kg)
mass_body = pIn.body_mass;

% Paddle speed spline (m/s)
sp_spd_pd = pIn.sp_spd_pd;

% Paddle angle spline (rad)
sp_ang_pd = pIn.sp_ang_pd;

% Time interval of cycle (s)
t_span = [pIn.t_events(1) pIn.t_events(3)];

% Time when power stroke ends (s)
t_end = pIn.t_events(2);

% Initial body speed (m/s)
U0 = pIn.U_initial;

clear pIn


%% General & calculated parameter values

% Body density (kg m^-3)
%rho_body  = 1030;

% Water density (kg m^-3)
rho_water = 1000;

% Projected area of body (m^2) -- calculation assumes same shape as Blake
S = pi*(0.2*L)^2;

% Initial body position (m)
X0 = 0;

% Height of appendage (m) -- calculation assumes same shape as Blake
h = 0.23.*L;

% Solver options
options    = odeset('RelTol',1e-7);


%% Scale parameters to non-dimensional units

% Define scaling constants
sL = L;
sT = 10^-3;
sM = mass_body*10^6;
sF = sM .* sL ./ sT^2;

% Declare global variables
%global p

% Scaled parameters
p.sL          = sL;
p.sT          = sT;
p.sM          = sM;
p.sF          = sF;

p.rho_water   = rho_water   /sM *sL^3;
p.mass_body   = mass_body   /sM;

p.L           = L           /sL;
p.U0          = U0          /sL *sT;
p.X0          = X0          /sL;
p.Cd_app      = Cd_app;
p.Cd_body     = Cd_body;

p.h           = h           /sL;
p.pLen        = pLen        /sL;
p.S           = S           /(sL^2);
p.t_span      = t_span ./sT;
p.t_end       = t_end/sT;

% Spline parameters (not scaled here)
p.sp_spd_pd   = sp_spd_pd;
p.sp_ang_pd   = sp_ang_pd;


%clear mu L k t_stop rho_body rho_water S Cd_app Cd_body


%% Run numerical simulation 

[t,X] = solver(p,options);


%% Store results (convert values back to SI units)


% TODO: Use the code below to calculate kinematics and trhust and drag
% TODO: Visualize comparison btwn model and data

% Calculate current paddle angle from spline
gama = fnval(p.sp_ang_pd,t.*p.sT);

% Calculate current paddle speed from spline
s_idx      = (t <= p.t_end);
v_n        = zeros(length(t),1);
v_n(s_idx) = fnval(p.sp_spd_pd,t(s_idx).*p.sT) ./p.sL .*p.sT;

% Thrust
thrust = app_thrust(gama,v_n,p,X(:,2));

% Drag
drag = body_drag(X(:,2),p);

% return a full set of result variables
r.v_n    = v_n     .*sL ./sT;
r.gamma  = gama;
r.thrust = thrust  .*sF;
r.drag   = drag    .*sF;
r.t      = t       .*sT;
r.x      = X(:,1)  .*sL;
r.U      = X(:,2)  .*sL  ./sT;

% Visualize results
if 0
    % Plot
    figure;
    subplot(3,1,1)
    [ax,h1,h2] = plotyy(r.t.*1000,r.gamma,r.t.*1000,r.v_n.*1000);
    ylabel(ax(1),'gamma (rad)')
    ylabel(ax(2),'v_n (mm/s)')
    grid on
    
    subplot(3,1,2)
    plot(r.t.*1000,r.thrust.*10^6,'g',r.t.*1000,r.drag.*10^6,'-')
    legend('thrust','drag')
    ylabel('Force (micro N)')
    grid on
    
    subplot(3,1,3)
    plot(r.t.*1000,r.U.*1000,'-')
    grid on
    ylabel('U')
    
end

clear U t X T D





end

function [t,X] = solver(p,options)

[t,X] = ode45(@gov_eqn,p.t_span,[p.X0; p.U0],options);

    function dX = gov_eqn(t,X)
        % ODE of the dynamics of the system
        
        % Body position
        pos = X(1);
        
        % Body speed
        U   = X(2);
       
        % Calculate current paddle angle from spline
        gama = fnval(p.sp_ang_pd,t.*p.sT);
        
        % Calculate current paddle speed from spline
        if t <= p.t_end
            v_n  = fnval(p.sp_spd_pd,t.*p.sT) ./p.sL .*p.sT;
        else 
            v_n = 0;
        end
        
        % Thrust
        thrust = app_thrust(gama,v_n,p,U);

        % Drag
        drag = body_drag(U,p);
        
        % Define output: speed
        dX(1,1) = U;
        
        % Define output: acceleration
        dX(2,1) = (thrust + drag) ./ p.mass_body;
        
    end

end

function thrust = app_thrust(gama,v_n,p,U)
% Calculate thrust generated by appendage

v_app = - v_n + U.*cos(gama-pi/2);

thrust = - cos(gama-pi/2)*....
                    0.5*p.rho_water.*p.Cd_app.*...
                    p.h.*p.pLen.*v_app.*abs(v_app);
     %TODO: subtract velocity from forward movement
     
% Adjust direction of thrust
%thrust = (gama-pi/2)./abs((gama-pi/2)) .* thrust;

end
  
function D = body_drag(U,p) 
% Calculates drag force on the body

D = -0.5 .* p.S .* p.rho_water .* p.Cd_body .* abs(U) .* U;

end

function y = spd_func(t,A,P,phs,rtrn_P,spd_0)
% Function that defines the speed of the power stroke

%phs = 0;
% Make sure t is a column vector
if size(t,2) > size(t,1)
    t = t';
end

% Define which stroke number we are within
stroke_num = floor(t/(P+rtrn_P))+1;

% Time adjusted to stroke cycle
t_ad = (t - ((stroke_num-1) * (P+rtrn_P)));

% Index of power stroke times
idx = t_ad <  (P - 0*phs);

% Power stroke velocity values
y(idx,1) = spd_0 + A.*sin(pi.*(t_ad(idx)-phs)./(1.5*P)).^2;

% Zero values for recovery stroke
y(~idx,1) = zeros(size(sum(~idx),1),size(sum(~idx),2));

end

function y = ang_func(t,A,P,ang_start,phs,rtrn_P)
% Defines angle of power stroke
%phs=0;
% Make sure t is a column vector
if size(t,2) > size(t,1)
    t = t';
end

% Define which stroke number we are within
stroke_num = floor(t/(P+rtrn_P))+1;

% Time adjusted to stroke cycle
t_ad = (t - ((stroke_num-1) * (P+rtrn_P)));

% Index of power stroke times
idx = t_ad <  (P - 0*phs);

% Angle for power stroke
y(idx,1) = ang_start + A.*sin(pi.*(t_ad(idx))./(2.*(1.1*P))).^2;

% Zero values for recovery stroke
y(~idx,1) = zeros(size(sum(~idx),1),size(sum(~idx),2));

end
